#!/usr/bin/env python3
"""
RK3588硬件编码视频流客户端 - Flask优化版
支持MPP硬件H.264编码和MJPEG编码
"""
import socket
import json
import subprocess
import time
import threading
import logging
import serial
import struct
import math
from datetime import datetime

logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)


class LD14PLidarReader:
    """LD14P激光雷达数据读取器"""
    
    def __init__(self, device="/dev/ttyACM0", baudrate=230400):
        self.device = device
        self.baudrate = baudrate
        self.serial_port = None
        self.running = False
        self.latest_scan = None
        
    def connect(self):
        """连接激光雷达"""
        try:
            self.serial_port = serial.Serial(
                port=self.device,
                baudrate=self.baudrate,
                bytesize=8,
                parity='N',
                stopbits=1,
                timeout=1.0
            )
            logger.info(f"激光雷达连接成功: {self.device}")
            return True
        except Exception as e:
            logger.error(f"激光雷达连接失败: {e}")
            return False
    
    def parse_lidar_data(self, data):
        """解析激光雷达数据包"""
        try:
            # LD14P数据包格式解析 (简化版本)
            # 实际格式需要根据厂商协议文档调整
            points = []
            
            # 假设数据包格式: [header][angle][distance][quality]...
            # 这里需要根据实际LD14P协议调整
            for i in range(0, len(data) - 6, 6):
                if i + 6 <= len(data):
                    # 解析角度和距离 (示例格式)
                    angle_raw = struct.unpack('<H', data[i:i+2])[0]
                    distance_raw = struct.unpack('<H', data[i+2:i+4])[0]
                    quality = struct.unpack('<H', data[i+4:i+6])[0]
                    
                    # 转换为实际值
                    angle = (angle_raw / 100.0) % 360.0  # 角度
                    distance = distance_raw / 1000.0     # 距离(米)
                    
                    if distance > 0.1 and distance < 8.0 and quality > 10:
                        # 转换为笛卡尔坐标
                        angle_rad = math.radians(angle)
                        x = distance * math.cos(angle_rad)
                        y = distance * math.sin(angle_rad)
                        
                        points.append({
                            'angle': round(angle, 2),
                            'distance': round(distance, 3),
                            'x': round(x, 3),
                            'y': round(y, 3),
                            'quality': quality
                        })
            
            return points
        except Exception as e:
            logger.error(f"激光雷达数据解析错误: {e}")
            return []
    
    def read_scan(self):
        """读取一次完整扫描"""
        if not self.serial_port:
            return None
            
        try:
            # 读取数据 (简化版本)
            data = self.serial_port.read(1000)  # 读取1KB数据
            if data:
                points = self.parse_lidar_data(data)
                if points:
                    scan_data = {
                        'timestamp': time.time(),
                        'points': points,
                        'point_count': len(points)
                    }
                    self.latest_scan = scan_data
                    return scan_data
        except Exception as e:
            logger.error(f"激光雷达读取错误: {e}")
        
        return None
    
    def start_reading(self):
        """开始读取激光雷达数据"""
        self.running = True
        
    def stop_reading(self):
        """停止读取激光雷达数据"""
        self.running = False
        if self.serial_port:
            self.serial_port.close()
            self.serial_port = None


class RK3588VideoClient:
    def __init__(self, server_ip="192.168.233.187", server_port=9000, lidar_port=9001):
        self.server_ip = server_ip
        self.server_port = server_port
        self.lidar_port = lidar_port
        self.camera_device = "/dev/video41"
        self.lidar_device = "/dev/ttyACM0"
        
        # 视频编码进程
        self.h264_process = None
        self.mjpeg_process = None
        
        # 激光雷达
        self.lidar = LD14PLidarReader(self.lidar_device)
        
        # 客户端状态
        self.running = False
        self.connected = False
        
    def check_camera_device(self):
        """检查摄像头设备"""
        import os
        
        if not os.path.exists(self.camera_device):
            logger.error(f"[错误] 摄像头设备不存在: {self.camera_device}")
            return False
        
        # 设置权限
        try:
            subprocess.run(['chmod', '666', self.camera_device], check=False)
            logger.info(f"[成功] 摄像头设备准备就绪: {self.camera_device}")
            return True
        except Exception as e:
            logger.warning(f"[警告] 设置设备权限失败: {e}")
            return True  # 继续尝试
    
    def cleanup_ports(self):
        """清理占用的端口"""
        subprocess.run(['pkill', '-f', 'gst-launch'], capture_output=True)
        subprocess.run(['pkill', '-f', 'tcpserversink'], capture_output=True)
        # 清理可能占用8080端口的进程
        subprocess.run(['fuser', '-k', '8080/tcp'], capture_output=True)
        logger.info("清理端口完成")
    
    def start_mjpeg_stream(self):
        """启动MJPEG视频流 - 极致纯传输优化"""
        logger.info("启动极致纯传输MJPEG视频流...")
        
        # 回到稳定配置 - 确保画面正常
        cmd = [
            'gst-launch-1.0', '-e',
            # 摄像头源 - 稳定配置
            'v4l2src', f'device={self.camera_device}',
            # 稳定格式
            '!', 'video/x-raw,width=640,height=480,framerate=15/1',
            # 视频转换
            '!', 'videoconvert',
            # JPEG编码 - 稳定质量
            '!', 'jpegenc', 'quality=75',
            # MJPEG封装
            '!', 'multipartmux', 'boundary=frame',
            # TCP传输 - 稳定传输
            '!', 'tcpserversink', 'host=0.0.0.0', 'port=8080', 
            'sync=false', 'async=false'
        ]
        
        try:
            logger.info("启动GStreamer管道...")
            
            self.mjpeg_process = subprocess.Popen(
                cmd,
                stdout=subprocess.PIPE,
                stderr=subprocess.PIPE
            )
            
            # 等待GStreamer启动
            time.sleep(2)
            
            if self.mjpeg_process.poll() is None:
                logger.info("MJPEG视频流启动成功")
                logger.info("视频地址: http://192.168.233.110:8080")
                return True
            else:
                # 获取错误信息
                stdout, stderr = self.mjpeg_process.communicate()
                logger.error(f"GStreamer启动失败: {stderr.decode('utf-8')[:200]}")
                return False
                
        except Exception as e:
            logger.error(f"MJPEG启动异常: {e}")
            return False
    
    def start_h264_stream(self):
        """启动MPP硬件H.264视频流 - 简化版"""
        logger.info("检查MPP硬件H.264支持...")
        
        # 由于MPP编码器经常不可用，直接跳过以节省时间
        logger.warning("跳过H.264编码器 - 专注MJPEG传输")
        return False
    
    def connect_to_server(self):
        """连接到Windows服务器"""
        try:
            self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.sock.settimeout(10)
            self.sock.connect((self.server_ip, self.server_port))
            self.connected = True
            logger.info(f"连接Windows服务器成功: {self.server_ip}:{self.server_port}")
            return True
        except Exception as e:
            logger.error(f"连接服务器失败: {e}")
            self.connected = False
            return False
    
    def send_video_status(self):
        """发送视频状态数据"""
        if not self.connected:
            return False
        
        try:
            # 检查编码器状态
            h264_running = self.h264_process and self.h264_process.poll() is None
            mjpeg_running = self.mjpeg_process and self.mjpeg_process.poll() is None
            
            # 确定当前协议和编码器
            if h264_running:
                protocol = 'H.264'
                codec = 'H.264 (MPP Hardware)'
                resolution = '1280x720'
                fps = 30
                bitrate = '2.0 Mbps'
                latency = '50-120ms'
            elif mjpeg_running:
                protocol = 'MJPEG'
                codec = 'MJPEG (USB Camera)'
                resolution = '640x480'
                fps = 15
                bitrate = '1.5 Mbps'
                latency = '100-250ms'
            else:
                protocol = 'None'
                codec = 'Offline'
                resolution = 'Unknown'
                fps = 0
                bitrate = 'Unknown'
                latency = 'Unknown'
            
            # 构建状态消息
            status_msg = {
                'type': 'video',
                'timestamp': datetime.now().isoformat(),
                'data': {
                    'connected': h264_running or mjpeg_running,
                    'status': 'online' if (h264_running or mjpeg_running) else 'offline',
                    'protocol': protocol,
                    'codec': codec,
                    'resolution': resolution,
                    'fps': fps,
                    'bitrate': bitrate,
                    'latency': latency
                }
            }
            
            # 发送数据
            json_data = json.dumps(status_msg) + '\n'
            self.sock.send(json_data.encode('utf-8'))
            
            return True
            
        except Exception as e:
            logger.error(f"发送状态失败: {e}")
            self.connected = False
            return False
    
    def send_heartbeat(self):
        """发送心跳包"""
        if not self.connected:
            return False
        
        try:
            heartbeat = {
                'type': 'heartbeat',
                'timestamp': datetime.now().isoformat(),
                'data': {'status': 'alive'}
            }
            
            json_data = json.dumps(heartbeat) + '\n'
            self.sock.send(json_data.encode('utf-8'))
            return True
            
        except Exception as e:
            logger.error(f"心跳发送失败: {e}")
            self.connected = False
            return False
    
    def send_lidar_data(self):
        """发送激光雷达数据"""
        if not self.connected or not self.lidar.running:
            return False
            
        try:
            # 读取激光雷达数据
            scan_data = self.lidar.read_scan()
            if scan_data and scan_data['points']:
                lidar_message = {
                    'type': 'lidar_data',
                    'timestamp': scan_data['timestamp'],
                    'points': scan_data['points'],
                    'point_count': scan_data['point_count'],
                    'device': self.lidar_device
                }
                
                json_data = json.dumps(lidar_message) + '\n'
                self.sock.send(json_data.encode('utf-8'))
                return True
            
        except Exception as e:
            logger.error(f"激光雷达数据发送失败: {e}")
            return False
    
    def data_sender_loop(self):
        """轻量级状态监控线程 - 最小化干扰"""
        logger.info("启动轻量级监控线程")
        
        loop_count = 0
        while self.running:
            try:
                # 如果连接断开，尝试重连
                if not self.connected:
                    if self.connect_to_server():
                        logger.info("重新连接成功")
                    else:
                        time.sleep(10)  # 重连失败等待更久
                        continue
                
                # 每60秒发送一次状态（大幅降低频率）
                if loop_count % 12 == 0:  # 12 * 5秒 = 60秒
                    if self.send_video_status():
                        logger.info(f"状态: {datetime.now().strftime('%H:%M:%S')}")
                
                # 每30秒发送一次心跳
                elif loop_count % 6 == 0:  # 6 * 5秒 = 30秒
                    self.send_heartbeat()
                
                # 每次循环都尝试发送激光雷达数据 (5Hz频率)
                self.send_lidar_data()
                
                loop_count += 1
                # 统一等待5秒 - 减少系统调用
                time.sleep(5)
                
            except Exception as e:
                logger.error(f"监控线程错误: {e}")
                self.connected = False
                time.sleep(10)
    
    def start(self):
        """启动RK3588视频客户端"""
        logger.info("启动RK3588硬件编码视频流客户端")
        logger.info(f"目标服务器: {self.server_ip}:{self.server_port}")
        logger.info(f"摄像头设备: {self.camera_device}")
        
        # 环境检查
        if not self.check_camera_device():
            return False
        
        # 清理端口
        self.cleanup_ports()
        
        # 启动视频编码流 - 专注MJPEG
        logger.info("启动视频编码器...")
        
        # 直接启动MJPEG流（H.264不稳定，已禁用）
        mjpeg_ok = self.start_mjpeg_stream()
        
        if not mjpeg_ok:
            logger.error("MJPEG视频流启动失败")
            return False
        
        # 连接Windows服务器
        if not self.connect_to_server():
            logger.error("无法连接Windows服务器")
            return False
        
        self.running = True
        
        # 启动激光雷达
        if self.lidar.connect():
            self.lidar.start_reading()
            logger.info("激光雷达启动成功")
        else:
            logger.warning("激光雷达启动失败，继续运行视频流")
        
        # 启动数据发送线程
        data_thread = threading.Thread(target=self.data_sender_loop, daemon=True)
        data_thread.start()
        
        logger.info("RK3588纯传输客户端启动成功")
        logger.info(f"Web监控界面: http://192.168.233.187:5000")
        logger.info("MJPEG流: http://192.168.233.110:8080")
        logger.info("纯传输模式 - 下位机专注数据传输")
        logger.info("状态更新频率: 60秒，心跳: 30秒")
        logger.info("按 Ctrl+C 停止客户端")
        
        try:
            while self.running:
                time.sleep(1)
        except KeyboardInterrupt:
            logger.info("收到停止信号")
        finally:
            self.stop()
        
        return True
    
    def stop(self):
        """停止客户端"""
        logger.info("正在停止RK3588客户端...")
        self.running = False
        
        # 停止激光雷达
        if self.lidar:
            self.lidar.stop_reading()
            logger.info("激光雷达已停止")
        
        # 停止视频编码进程
        if self.h264_process:
            self.h264_process.terminate()
            logger.info("H.264编码器已停止")
            
        if self.mjpeg_process:
            self.mjpeg_process.terminate()
            logger.info("MJPEG编码器已停止")
        
        # 关闭网络连接
        if hasattr(self, 'sock'):
            self.sock.close()
        
        logger.info("RK3588客户端已完全停止")

if __name__ == "__main__":
    import sys
    
    # 解析命令行参数
    if len(sys.argv) >= 3:
        server_ip = sys.argv[1]
        server_port = int(sys.argv[2])
    elif len(sys.argv) == 2:
        server_ip = sys.argv[1]
        server_port = 9000
    else:
        server_ip = "192.168.233.187"
        server_port = 9000
        
    logger.info(f"使用配置: {server_ip}:{server_port}")
    
    # 启动客户端
    client = RK3588VideoClient(server_ip, server_port)
    client.start()